/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package control;

import world.Point;

/**
 * 
 * @author dheath
 */
public class Utility
{
    /*
     * contains methods to calculate actions for bot location detection, etc.
     */
    // private methods

    // public static methods
    public static double angleDiff(double angle, Point a, Point b, long DT)
    {
        double targangle = Math.atan2(b.getY() - a.getY(), b.getX() - a.getX());

        if (targangle < 0)
        {
            targangle = 2 * Math.PI + targangle;
        }

        double diff = targangle - angle;


        if (diff < 0)
        {
            diff += Math.PI * 2;
        }


        //System.out.println("target: "+radianToDegrees(targangle)+" tank: "+radianToDegrees(angle)+" diff: "+radianToDegrees(diff));
        double err = 0.0;

        if (diff <= Math.PI)
        {
            //angvel = diff / Math.PI;//*diff;
            err = diff;

        } else
        {
            //angvel = -1 * (((Math.PI * 2) - diff) / Math.PI);//*(((Math.PI * 2) - diff));
            err = Math.PI * 2 - diff;
        }
        
        return err;
    }

    public static double root(double A, double B, double C) throws IllegalArgumentException
    {
        // Returns the larger of the two roots of
        // the quadratic equation A*x*x + B*x + C = 0.
        // (Throws an exception if A == 0 or B*B-4*A*C < 0.)
        if (A == 0)
        {
            System.out.println("err");
            return 0;
        } else
        {
            double disc = B * B - 4 * A * C;
            if (disc < 0)
            {
                System.out.println("err");
                return 0;
            }

            //System.out.println((-B + Math.sqrt(disc)) / (2 * A) + " and "+(-B - Math.sqrt(disc)) / (2 * A));
            return (-B + Math.sqrt(disc)) / (2 * A);
        }
    } // end root()

    public static double distance(Point a, Point b)
    {
        return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
    }

    public static double radianToDegrees(double angle)
    {
        return angle * 180 / Math.PI;
    }
    public static double degreeToRadians(double degrees) 
    {
    	return degrees * Math.PI / 180;
    	
    }

    public static Point minus(Point a, Point b)
    {
        return new Point(a.getX() - b.getX(), a.getY() - b.getY());
    }

    public static Point plus(Point a, Point b)
    {
        return new Point(a.getX() + b.getX(), a.getY() + b.getY());
    }

    public static double dot(Point a, Point b)
    {
        return a.getX() * b.getX() + a.getY() * b.getY();
    }

    public static double velocity(Point p1, Point p2, double timeElapsedms)
    {
        // returns the velocity, in world coordinate units per second, of how
        // fast a bot traveled from p1 to p2
        return distance(p1, p2) / (.001 * timeElapsedms);
    }

    public static double acceleration(double vel1, double vel2, double deltatms)
    {
        // returns the acceleration in world coordinate units per second
        // squared,
        // of a bot that has changed from velocity vel1 to vel2 in deltatms
        // milliseconds
        return (vel2 - vel1) / (.001 * deltatms);
    }

    public static double angularVelocity(double ang1, double ang2,
            double intervalms)
    {
        // returns the angular velocity in radians per second.
        // takes ang1, the original angle of a tank,
        // ang2, the final angle of a tank,
        // intervalms, which is the time (in milliseconds) it took for a tank to
        // turn that far (we'll assume that it hasn't turned more than one
        // circle in an interval)

        // returns angle between ang1 and ang2
        double theta = Math.acos(getXComponentNorm(ang1) * getXComponentNorm(ang2) + getYComponentNorm(ang1) * getYComponentNorm(ang2));

        return theta / (.001 * intervalms);
    }

    public static double averageAcceleration(Point[] points,
            double[] timeintervals, int numPoints)
    {
        // returns the average acceleration in world coordinate units per second
        // squared,
        // that has numPoints points and has numPoints-1 timeIntervals, that
        // correspond to the time intervals between the points.
        double totalAccel = 0;

        for (int i = 0; i < numPoints - 2; i++)
        {
            Point p0 = points[i];
            Point p1 = points[i + 1];
            double tElapsedms0 = timeintervals[i];
            double vel0 = velocity(p0, p1, tElapsedms0);
            Point p2 = points[i + 2];
            double tElapsedms1 = timeintervals[i + 1];
            double vel1 = velocity(p1, p2, tElapsedms1);
            totalAccel += acceleration(vel0, vel1, timeintervals[i + 1]);
        }
        double acc = totalAccel / (numPoints - 2);
        return acc;
    }

    public static double getAngleRadians(Point p1, Point p2)
    {
        // gets the angle from point p1 to p2 in radians
        return Math.atan2(p2.getY() - p1.getY(), p2.getX() - p1.getX());
    }

    public static double getXComponentNorm(Point p1, Point p2)
    {
        // gets the normalized x component of the line from p1 to p2
        return (p2.getX() - p1.getX()) / distance(p1, p2);
    }

    public static double getYComponentNorm(Point p1, Point p2)
    {
        // gets the normalized y component of the line from p1 to p2
        return (p2.getY() - p1.getY()) / distance(p1, p2);
    }

    public static double getXComponentNorm(double radians)
    {
        // gets the normalized x component corresponding to the angle specified
        // by radians
        double val = (radians >= 0) ? radians : (2 * Math.PI - radians);
        return Math.cos(val);
    }

    public static double getYComponentNorm(double radians)
    {
        // gets the normalized y component corresponding to the angle specified
        // by radians
        double val = (radians >= 0) ? radians : (2 * Math.PI - radians);
        return Math.sin(val);
    }
    public static double getRadians02PI(double radians)
    {
    	if (radians >= 2 * Math.PI)
    	{
    		radians -= 2 * Math.PI;
    	}
    	if (radians < 0) 
    	{
    		radians = 2 * Math.PI - radians;
    	}
    	return radians;
    }
}
